#include "multitasker.h" 
#include "paging.h" 
#include "malloc.h" 

freeBlock_t firstFreeBlock;

void init_heap2(){
  
  
  	firstFreeBlock = (freeBlock_t) HEAP_PAGETABLE;
	firstFreeBlock->prev = NULL;
	firstFreeBlock->size = sizeof(struct freeBlock_t);
	
	freeBlock_t secondBlock = (freeBlock_t)((unsigned int)firstFreeBlock + firstFreeBlock->size); 
	secondBlock->prev = firstFreeBlock;
	secondBlock->size = PAGESIZE - 2*sizeof(struct freeBlock_t);
	secondBlock->next = NULL;
	
	firstFreeBlock->next = secondBlock;
  
}

void infoMem(void * ptr){

	block_t block = (block_t)((unsigned int)ptr - sizeof(struct block_t));
	
	printk("Pos:%d\n",block);
	printk("Size:%d\n",block->size);
	printk("TEXT:%s\n",block->ptr);
}


void printLibres(void){
	
	freeBlock_t freeBlock = firstFreeBlock;
	int i=1;
	
	while( freeBlock != NULL ){
printk("FREE(%d)---> Pos:%d\t",i, freeBlock);
printk("Next:%d\t",freeBlock->next);
printk("Size:%d\n",freeBlock->size);
			freeBlock = freeBlock->next;
			i++;
	}
}

int _reAgroupBlocks(freeBlock_t firstBlock){
	
	
	freeBlock_t secondBlock = firstFreeBlock;
	
	unsigned int wantedPos = (unsigned int)firstBlock + firstBlock->size;
	
	secondBlock = secondBlock->next;
	while( secondBlock != NULL && ((unsigned int)secondBlock != wantedPos) ){
		secondBlock = secondBlock->next;
	}
	
	if((unsigned int)secondBlock == wantedPos){
//printLibres();		
//printk("REAGRUPANDO\n\n");
		freeBlock_t preFirstBlock = (freeBlock_t)firstBlock->prev;
		freeBlock_t posFirstBlock = (freeBlock_t)firstBlock->next;
		
		preFirstBlock->next=posFirstBlock;
		posFirstBlock->prev=preFirstBlock;
		
//printk("preFirstBlock->next=%d\n", preFirstBlock->next);
//printk("posFirstBlock->prev=%d\n", posFirstBlock->prev);			
		
		freeBlock_t preSecondBlock = (freeBlock_t)secondBlock->prev;
		freeBlock_t posSecondBlock = (freeBlock_t)secondBlock->next;
		
		preSecondBlock->next=posSecondBlock;
		posSecondBlock->prev=preSecondBlock;
		
		int firstBlockSize = firstBlock->size;
		int secondBlockSize = secondBlock->size;
		
//printk("preFirstBlock->next=%d\n", preFirstBlock->next);
//printk("posFirstBlock->prev=%d\n", posFirstBlock->prev);

		block_t newFreeBlock = (block_t)firstBlock;
		newFreeBlock->size = firstBlockSize + secondBlockSize;		
		newFreeBlock->ptr = (void *)((unsigned int)newFreeBlock + sizeof(struct block_t));
		freeMem(newFreeBlock->ptr);		

//printLibres();
		return 1;
	}
	return 0;
}



void reAgroupBlocks(void){
	
	freeBlock_t freeBlock = firstFreeBlock->next;	
	int change = 0;
	while( freeBlock != NULL && !change){
		change = _reAgroupBlocks(freeBlock);
		freeBlock = freeBlock->next;		
	}	
}


void freeMem(void * ptr){
	
//	reAgroupBlocks();
//printLibres();
	
	block_t block = (block_t)((unsigned int)ptr - sizeof(struct block_t));
	
//printk("FREE1---> Pos:%d\n",block);
//printk("FREE2---> Size:%d\n",block->size);
//printk("FREE3---> TEXT:%s\n",block->ptr);
	
	freeBlock_t freeBlock = firstFreeBlock;
	
	while( (freeBlock->next != NULL) && (freeBlock->size < block->size) ){
//		printk("freeBlock->next == NULL\n");
		freeBlock = freeBlock->next;
	}
	
	if(freeBlock->size > block->size)
		freeBlock = freeBlock->prev;
	
	unsigned int size = block->size;
	
	freeBlock_t newFreeBlock = (freeBlock_t)block;	
	
	newFreeBlock->size = size;
	
	newFreeBlock->next = freeBlock->next;
	
	((freeBlock_t)freeBlock->next)->prev = newFreeBlock;
		
	newFreeBlock->prev = freeBlock;
	
	freeBlock->next = newFreeBlock;
	
//	printk("fistfreeblock: %d\n",firstFreeBlock);
//	printk("fistfreeblock->next: %d\n",firstFreeBlock->next);

	reAgroupBlocks();
	
}

void* _malloc2(freeBlock_t freeBlock, unsigned int size){
//	printk("freeBlock pos:%d\n", freeBlock);
//	printk("freeBlock->next:%d\n", freeBlock->next);
//	printk("freeBlock->size:%d\n", freeBlock->size);
	
	while(freeBlock != NULL && freeBlock->size < size){
		freeBlock = freeBlock->next;
		
	}
	
	if(freeBlock == NULL){
		
		unsigned int newPage = grow_heap();
		
		block_t block = (block_t)newPage;
		block->size=4096;
		
		freeMem((void*)((unsigned int)block + sizeof(struct block_t)));
		return  _malloc2(firstFreeBlock, size);
		
	}
	
	freeBlock_t prev = freeBlock->prev;
	
	unsigned int marginSize = 3 + sizeof(struct freeBlock_t); marginSize >>= 2; marginSize <<= 2;

	if( (freeBlock->size - size) <= marginSize){	
		
		freeBlock_t preFreeBlock = (freeBlock_t)freeBlock->prev;
		freeBlock_t posFreeBlock = (freeBlock_t)freeBlock->next;
		unsigned int freeBlockSize = freeBlock->size;
		
		preFreeBlock->next=posFreeBlock;
		posFreeBlock->prev=preFreeBlock;
		
		block_t ret = (block_t)(freeBlock);
		ret->ptr = (void *)((unsigned int)ret + sizeof(struct block_t));
		ret->size = freeBlockSize;
		
		return ret->ptr;
	}		
	
	freeBlock_t newFreeBlock = (freeBlock_t) ((unsigned int)freeBlock +  size);
	newFreeBlock->next =freeBlock->next;
	newFreeBlock->prev = freeBlock->prev;
	newFreeBlock->size = freeBlock->size - size;
	

//	printk("NEW FREE BLOCK POS:%d\n", newFreeBlock);
//	printk("NEW FREE BLOCK SIZE:%d\n", newFreeBlock->size);
	
	block_t ret = (block_t)(freeBlock);
	ret->ptr = (void *)((unsigned int)ret + sizeof(struct block_t));
	ret->size = size;
	
	if(prev != NULL)
			prev->next = newFreeBlock;
		else
			firstFreeBlock = newFreeBlock;
		
	return (void *)(ret->ptr);

}


void* malloc(unsigned int size){	
	
	size += 3 + sizeof(struct freeBlock_t);
	
	size >>= 2;
	size <<= 2;


	return (void*) _malloc2(firstFreeBlock, size);
}
